#include <im_joint_state_publisher/im_joint.h>

namespace im_joint_state_publisher {
ImJoint::ImJoint(boost::shared_ptr<const urdf::Joint> joint,
		interactive_markers::InteractiveMarkerServer* server,
		double marker_size,
		interactive_markers::InteractiveMarkerServer::FeedbackCallback callBack) {
	joint_name = joint->name;
	parent_name = joint->parent_link_name;
	mimic_name = "";
	urdf::Vector3 o = joint->parent_to_joint_origin_transform.position;
	urdf::Rotation r = joint->parent_to_joint_origin_transform.rotation;
	joint_angle = 0;
	scale = marker_size;

	if (joint->type == urdf::Joint::UNKNOWN
			|| joint->type == urdf::Joint::FLOATING
			|| joint->type == urdf::Joint::PLANAR
			|| joint->type == urdf::Joint::FIXED) {
		type = FIXED;
		ROS_INFO("joint created with name->%s	parent->%s	has been forced to be type->%d", joint_name.c_str(), parent_name.c_str(), type);
	} else {
		if (joint->type == urdf::Joint::REVOLUTE) {
			type = REVOLUTE;
			upper_limit = joint->limits->upper;
			lower_limit = joint->limits->lower;
		} else if (joint->type == urdf::Joint::CONTINUOUS) {
			type = CONTINUOUS;
			upper_limit = M_PI;
			lower_limit = -M_PI;
		} else if (joint->type == urdf::Joint::PRISMATIC) {
			type = PRISMATIC;
			upper_limit = joint->limits->upper;
			lower_limit = joint->limits->lower;
		}
		origin = tf::Vector3(o.x, o.y, o.z);
		rotation = tf::Quaternion(r.x, r.y, r.z, r.w);
		joint_axis.x = joint->axis.x;
		joint_axis.y = joint->axis.y;
		joint_axis.z = joint->axis.z;
		urdf::JointMimic *mimic = joint->mimic.get();
		if (mimic != NULL) {
			mimic_name = mimic->joint_name;
			multiplier = mimic->multiplier;
			offset = mimic->offset;
			ROS_INFO("Mimic found");
		} else
			makeFrame(server, callBack);
		ROS_INFO("joint created with name->%s	parent->%s	type->%d axis x=%f y=%f z=%f origin x=%f y=%f z=%f rotation x=%f y=%f z=%f w=%f ", joint_name.c_str(), parent_name.c_str(), type, joint_axis.x, joint_axis.y, joint_axis.z, origin.x(), origin.y(), origin.z(), rotation.x(), rotation.y(), rotation.z(), rotation.w());
	}
	////////
}
void ImJoint::makeFrame(interactive_markers::InteractiveMarkerServer* server,
		interactive_markers::InteractiveMarkerServer::FeedbackCallback callBack) {
	visualization_msgs::InteractiveMarker frame_marker;
	std::string str = "/";
	str += parent_name;
	frame_marker.header.frame_id = joint_name;
	frame_marker.name = joint_name;
	frame_marker.description = joint_name;
	frame_marker.scale = scale;
	if (type == REVOLUTE || type == CONTINUOUS)
		add_rotation(&frame_marker);
	else
		add_translation(&frame_marker);
	server->insert(frame_marker);
	server->setCallback(frame_marker.name, callBack, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
}

void ImJoint::add_rotation(visualization_msgs::InteractiveMarker *frame_marker) {
	visualization_msgs::InteractiveMarkerControl Frame_control;
	Frame_control.orientation.w = 1;
	Frame_control.orientation.x = joint_axis.x;
	Frame_control.orientation.y = joint_axis.z;
	Frame_control.orientation.z = joint_axis.y;
	Frame_control.always_visible = false;
	Frame_control.name = "rotate";
	Frame_control.interaction_mode =visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	frame_marker->controls.push_back(Frame_control);
}

void ImJoint::add_translation(
		visualization_msgs::InteractiveMarker *frame_marker) {
	visualization_msgs::Marker Y;
	visualization_msgs::InteractiveMarkerControl Frame_control;
	Y.type = visualization_msgs::Marker::ARROW;
	Y.scale.x = scale;
	Y.scale.y = scale;
	Y.scale.z = scale;
	Y.color.r = 0.0;
	Y.color.g = 1.0;
	Y.color.b = 0.0;
	Y.color.a = 1.0;
	Y.pose.orientation.w = 1;
	Y.pose.orientation.x = joint_axis.x;
	Y.pose.orientation.y = joint_axis.z;
	Y.pose.orientation.z = joint_axis.y;
	Frame_control.orientation.w = 1;
	Frame_control.orientation.x = joint_axis.x;
	Frame_control.orientation.y = joint_axis.z;
	Frame_control.orientation.z = joint_axis.y;
	Frame_control.name = "move";
	Frame_control.always_visible = false;
	Frame_control.interaction_mode =visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
	Frame_control.markers.push_back(Y);
	frame_marker->controls.push_back(Frame_control);
}

void ImJoint::setJointRotation(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
	ROS_DEBUG_STREAM(feedback->marker_name << "->" << feedback->pose.position.x << ", " << feedback->pose.position.y << ", " << feedback->pose.position.z<<", "<<feedback->pose.orientation.w<<", "<< feedback->pose.orientation.x <<", "<<feedback->pose.orientation.y<<", "<<feedback->pose.orientation.z);
	double angle, x, y, z;
	if (type == REVOLUTE || type == CONTINUOUS) {
		angle = 2 * acos(feedback->pose.orientation.w);
		x = joint_axis.x * feedback->pose.orientation.x;
		y = joint_axis.y * feedback->pose.orientation.y;
		z = joint_axis.z * feedback->pose.orientation.z;
	} else {
		double buf;
		buf = pow(feedback->pose.position.x, 2) + pow(feedback->pose.position.y, 2)+ pow(feedback->pose.position.z, 2);
		angle = sqrt(buf);
		x = joint_axis.x * feedback->pose.position.x;
		y = joint_axis.y * feedback->pose.position.y;
		z = joint_axis.z * feedback->pose.position.z;
	}

	if (x <= 0 && y <= 0 && z <= 0)
		angle = -angle;
	if (angle > upper_limit)
		angle = upper_limit;
	if (angle < lower_limit)
		angle = lower_limit;
	joint_angle = angle;
	ROS_INFO("Joint->%s 	angle(in degree)->%f", joint_name.c_str(), joint_angle*180.0/M_PI);
}
}
/// namespace im_joint_state_publisher
